classdef newPidControler  < handle
    %这个用的是增量式pid控制器
    properties
        u_now;     % 控制量，成为真正的控制输出
        ref_value;   %这个就是参考值，然后减去实际值就是err
        err_sum;    %求和的误差，用于增量式
        err_last;
        
        %params
        kp;
        kv;
        tv;
        tm;
        km;
        kd;
        kt;
        
        
        %id_params逆动力学的参数
        pid_kp;
        pid_ki;
        pid_kd;
        
        is_int_enable;
        is_feedforward_enable;
        is_torque_feedforward_enable;
        
        n_group;
    end
    
    methods
        function obj = newPidControler(sj_obj,motor_obj,ts,zeta,is_int_enable,is_feedforward_enable,is_torque_feedforward_enable)
            if(nargin >= 2 )
                obj.n_group = sj_obj.n_group;
                %先用sj初始化所有参数
                if(nargin >= 4)
%                     if (zeta == 1), wn = 4.75./ts; end % 临界阻尼系统的自然频率
%                     if (zeta < 1), wn = 3.5./(zeta.*ts); end % 欠阻尼系统的自然频率
%                     if (zeta > 1), wn = 3.3./((zeta- sqrt(zeta.^2-1)).*ts); end % 过阻尼系统的自然频率
%                 
%                             bad_rows = find(con_value(:,1)< -10,obj.n_group);
%             con_value(bad_rows,1) = -10;
%             bad_rows = find(con_value(:,1)> 10,obj.n_group);
%             con_value(bad_rows,1) = 10;
                    wn = zeros(obj.n_group,1);
                    rows = find(zeta(:,1)== 1,obj.n_group);
                    wn(rows,1) = 4.75./ts(rows,1);
                    rows = find(zeta(:,1)> 1,obj.n_group);
                    wn(rows,1) = 3.3./((zeta(rows,1)- sqrt(zeta(rows,1).^2-1)).*ts(rows,1));
                    rows = find(zeta(:,1)< 1,obj.n_group);
                    wn(rows,1) =  3.5./(zeta(rows,1).*ts(rows,1));
                    
                else
                    %使用默认的特性
                    ts = repmat(1,obj.n_group,1);%调节时间
                    zeta = ones(obj.n_group,1);%阻尼比
                    wn = 4.75./ts;
                end
                %以下全部使用力矩模式的值，因为题目如此要求
                obj.kt = motor_obj.Ka*motor_obj.Kg;
                obj.tm = sj_obj.f_Im./sj_obj.Bm; % 用理论等效惯量计算
                obj.km = obj.kt./sj_obj.Bm;
                obj.kd = 1./sj_obj.Bm;
                
                obj.tv = obj.tm;
                obj.kv = 2*zeta.*wn./obj.km;
                obj.kp = wn./(2*zeta);
                
                obj.pid_kp = (wn).^2;
                obj.pid_kd = 2.*wn.*zeta;
                obj.pid_ki = repmat(0.0007,obj.n_group,1);
                
                disp("using default attribute");
                [obj.u_now,obj.err_last,obj.err_sum,...
                    obj.is_int_enable,obj.is_feedforward_enable,...
                    obj.is_torque_feedforward_enable]...
                    = deal(zeros(obj.n_group,1));
                if(nargin == 5)
                    obj.is_int_enable = is_int_enable;
                elseif(nargin == 6)
                    obj.is_int_enable = is_int_enable;
                    obj.is_feedforward_enable = is_feedforward_enable;
                elseif(nargin == 7)
                    obj.is_int_enable = is_int_enable;
                    obj.is_feedforward_enable = is_feedforward_enable;
                    obj.is_torque_feedforward_enable = is_torque_feedforward_enable;
                end
            else
                ME = MException('need at least 2  param!');
                throw(ME)
            end
        end
        
        
        function pid_reset(obj)
            [obj.u_now,obj.err_last,obj.err_sum] = deal(zeros(obj.n_group,1));
        end
        
        function con_value = get_con_value(obj,sj_obj,actual_value,simu_period,torque)      
            
             %这个是获得累加式的控制值，每次要传入state-info，也就是三种量
            con_value = zeros(obj.n_group,1,4);%组数、一行、con, e_pos, err_vel,e_sum
            err_now = (obj.ref_value - actual_value).*sj_obj.n_factor;
            
            %注意这里的err-sum不是真的单纯期望减误差的累积！,而是err-vel的积分
            
            %先做位置P
            %这里别弄上限处理，因为不一定只考虑位置，没那么简单
%             bad_rows = find(err_now(:,1)< -pi,obj.n_group);
%             err_now(bad_rows,1) = mod(err_now(bad_rows,1),pi);
%             bad_rows = find(err_now(:,1)> pi,obj.n_group);
%             err_now(bad_rows,1) = mod(err_now(bad_rows,1),-pi);
            
            %con始终为列向量
            pos_con = obj.kp .* err_now(:,1) ;
            
            vel_err_now = pos_con+err_now(:,2);
            obj.err_sum = obj.err_sum + vel_err_now * simu_period;
            
            obj.u_now = obj.kv.*obj.tv .* vel_err_now + (obj.kv .* obj.err_sum).*obj.is_int_enable;       
            %还需要增加前馈部分
            obj.u_now = obj.u_now + (obj.ref_value(2).*sj_obj.n_factor./obj.km+...
                obj.ref_value(3).*sj_obj.n_factor.*obj.tm./obj.km)...
                .*obj.is_feedforward_enable...
                + obj.is_torque_feedforward_enable.*torque./obj.kt;
            
            
            con_value(:,1,3) =  obj.err_last;
            obj.err_last = vel_err_now;
            con_value(:,1,1) = obj.u_now;
            con_value(:,1,2) = err_now(:,1)./sj_obj.n_factor;
            con_value(:,1,4) = obj.err_sum;
            
        end
        
        function con_value = get_id_con_value(obj,sj_obj,motor_obj,simu_period)
           %这个是逆动力学控制器，假定用电机的力矩模式了，算出fc直接除系数
           %实际值还是传n*3
            con_value = zeros(obj.n_group,4);%组数、一行、con, e_pos, err_vel,e_sum
%             err_now = obj.ref_value - sj_obj.joint_state;
            err_now = (obj.ref_value - sj_obj.joint_state).*sj_obj.n_factor;
            obj.err_sum = obj.err_sum + err_now(:,1).*simu_period;
            acc_design = obj.ref_value(:,3).*sj_obj.n_factor+ obj.pid_kp.*err_now(:,1)...
                +(obj.pid_ki.*obj.err_sum(:,1)).*(obj.is_int_enable)+ ...
                obj.pid_kd.*(err_now(:,2));
            tmp = sj_obj.inverse_dynamic(1,sj_obj.joint_state,[sj_obj.state(:,1:2) acc_design]);
            con_value(:,1) = (tmp(:,1)+tmp(:,2)-tmp(:,3))./(motor_obj.Ka*motor_obj.Kg);
            
            %输出限幅
            bad_rows = find(con_value(:,1)< -10,sj_obj.n_group);
            con_value(bad_rows,1) = -10;
            bad_rows = find(con_value(:,1)> 10,sj_obj.n_group);
            con_value(bad_rows,1) = 10;
            
            con_value(:,2) = err_now(:,1);
            con_value(:,3) = err_now(:,2);
            con_value(:,4) = err_now(:,3);
        end
                
        
        
        
        
        function set_target(obj,ref_value)
            obj.ref_value = ref_value;            
        end
    end
     
end